#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
麦克纳姆轮小车 ROS2 驱动启动文件

该 launch 文件启动机器人驱动节点，包括：
- 串口通信
- 里程计发布 (/chassis/odom)
- IMU 数据发布 (/chassis/imu)
- 电池状态发布 (/chassis/battery_state)
- 速度指令订阅 (/chassis/cmd_vel)
- TF 变换 (odom -> base_link)

使用方法:
    ros2 launch robot_chassis robot_driver.launch.py
    ros2 launch robot_chassis robot_driver.launch.py serial_port:=/dev/ttyACM0
"""

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    # 获取包路径
    pkg_share = get_package_share_directory('robot_chassis')
    
    # 默认参数文件路径
    default_params_file = os.path.join(pkg_share, 'config', 'robot_params.yaml')
    
    # 声明启动参数
    serial_port_arg = DeclareLaunchArgument(
        'serial_port',
        default_value='/dev/ttyACM0',
        description='串口设备路径 (例如: /dev/ttyACM0, /dev/ttyACM0)'
    )
    
    baud_rate_arg = DeclareLaunchArgument(
        'baud_rate',
        default_value='115200',
        description='串口波特率'
    )
    
    timeout_arg = DeclareLaunchArgument(
        'timeout_ms',
        default_value='1000',
        description='串口超时时间 (毫秒)'
    )
    
    odom_frame_arg = DeclareLaunchArgument(
        'odom_frame_id',
        default_value='odom',
        description='里程计坐标系名称'
    )
    
    base_frame_arg = DeclareLaunchArgument(
        'base_frame_id',
        default_value='base_link',
        description='机器人基座坐标系名称'
    )
    
    publish_tf_arg = DeclareLaunchArgument(
        'publish_tf',
        default_value='true',
        description='是否发布 TF 变换'
    )
    
    params_file_arg = DeclareLaunchArgument(
        'params_file',
        default_value=default_params_file,
        description='参数文件路径'
    )
    
    # 机器人驱动节点
    robot_driver_node = Node(
        package='robot_chassis',
        executable='robot_chassis',
        name='robot_driver',
        output='screen',
        parameters=[
            LaunchConfiguration('params_file'),
            {
                'serial_port': LaunchConfiguration('serial_port'),
                'baud_rate': LaunchConfiguration('baud_rate'),
                'timeout_ms': LaunchConfiguration('timeout_ms'),
            }
        ],
        remappings=[
            # 如果需要重映射话题，可以在这里配置
            # ('odom', '/robot/odom'),
            # ('imu', '/robot/imu'),
        ]
    )
    
    return LaunchDescription([
        # 启动参数
        serial_port_arg,
        baud_rate_arg,
        timeout_arg,
        odom_frame_arg,
        base_frame_arg,
        publish_tf_arg,
        params_file_arg,
        
        # 节点
        robot_driver_node,
    ])

